//
// Created by lewis on 2023/12/20.
//

#include "mavlink/v2.0/standard/mavlink.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>

int main()
{
    printf("build_branch:\t%s\nbuild_sha1:\t%s\nbuild_time:\t%s %s\n",
           GIT_BRANCH_NAME, GIT_COMMIT_SHA1, __DATE__, __TIME__);
    int recv_port = 26540;

    // 创建UDP socket
    int recv_px4_sock_fd_ = socket(AF_INET, SOCK_DGRAM, 0);

    // 绑定本地地址和端口
    struct sockaddr_in st_recv_px4_addr_{};
    st_recv_px4_addr_.sin_family = AF_INET;
    st_recv_px4_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
    st_recv_px4_addr_.sin_port = htons(recv_port);

    if (bind(recv_px4_sock_fd_, (struct sockaddr*)&st_recv_px4_addr_, sizeof(struct sockaddr)) == -1)
    {
        std::cerr << "bind socket failed!!!" << std::endl;
        return -1;
    }

    char buffer[1024];
    socklen_t addr_len = sizeof(struct sockaddr);
    struct sockaddr_in px4_addr{};

    mavlink_message_t msg;
    mavlink_status_t mavlinkStatus;
    while(true)
    {
        ssize_t len = recvfrom(recv_px4_sock_fd_, buffer, sizeof(buffer), 0, (struct sockaddr *) &px4_addr, &addr_len);
        if (len > 0)
        {
            for (size_t i = 0; i < len; i++)
            {
                if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &mavlinkStatus))
                {
                    switch (msg.msgid)
                    {
                        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
                        {
                            mavlink_global_position_int_t global_pos_;
                            mavlink_msg_global_position_int_decode(&msg, &global_pos_);
                            std::cout << "global pos : lat = " << global_pos_.lat
                                      << ", lon = " << global_pos_.lon
                                      << ", alt = " << global_pos_.alt
                                      << std::endl;
                            break;
                        }
                        default:
                            break;
                    }
                }
            }
        }
        usleep(1*1000);
    }
    return 0;
}